#include "servo.h"

// ==================== PWM设置函数 ====================
void Servo_PWM_Setup(TIM_HandleTypeDef htim, uint32_t Channel, uint32_t freq, uint16_t duty)
{
    uint16_t prescaler = TIMER_CLK_FREQ / 1000000UL - 1;                      // 预分频，使计数频率1MHz
    float pwm_period_arr = (1000000UL * 1.0f) / freq - 1;                     // ARR
    float pwm_duty_pulse = duty *1.0f / DUTY_MAX* (pwm_period_arr + 1); // CCR

    __HAL_TIM_SET_PRESCALER(&htim, prescaler);
    __HAL_TIM_SetAutoreload(&htim, (uint32_t)pwm_period_arr);
    __HAL_TIM_SetCompare(&htim, Channel, (uint32_t)pwm_duty_pulse);
}

void Servo_Duty_Setup(TIM_HandleTypeDef htim, uint32_t Channel, uint16_t duty)
{
    float pwm_period_arr = (1000000UL * 1.0f) / PWM_DEFAULT_FREQ - 1;         // ARR
    float pwm_duty_pulse = duty * 1.0f / DUTY_MAX*  (pwm_period_arr + 1); // CCR
    __HAL_TIM_SetCompare(&htim, Channel, (uint32_t)pwm_duty_pulse);           // 只设置电机占空比
}

// ==================== 舵机初始化函数 ====================
void Servo_Init(Servo_TypeDef *servo, TIM_HandleTypeDef *htim, uint32_t channel)
{
    // 初始化舵机结构体
    servo->htim = htim;
    servo->channel = channel;
    servo->min_angle = 0;
    servo->max_angle = 180;
    servo->min_pulse = SERVO_MIN_PULSE_WIDTH;
    servo->max_pulse = SERVO_MAX_PULSE_WIDTH;
    servo->current_angle = 90; // 默认中间位置
    servo->speed = 5;          // 默认速度
    servo->initialized = 0;
        // 启动PWM输出
    HAL_TIM_PWM_Start(htim, channel);
    // 初始化PWM频率为50Hz（舵机标准频率）
    Servo_PWM_Setup(*htim, channel, PWM_DEFAULT_FREQ, 0);

    servo->initialized = 1;
    // 将舵机移动到中间位置
    Servo_SetAngle(servo, 90);

    
}

// ==================== 舵机限位设置函数 ====================
void Servo_SetLimits(Servo_TypeDef *servo, uint16_t min_angle, uint16_t max_angle,
                     uint16_t min_pulse, uint16_t max_pulse)
{
    servo->min_angle = min_angle;
    servo->max_angle = max_angle;
    servo->min_pulse = min_pulse;
    servo->max_pulse = max_pulse;
}

// ==================== 舵机角度控制函数 ====================
void Servo_SetAngle(Servo_TypeDef *servo, uint16_t angle)
{
    if (!servo->initialized)
    {
        return; // 如果舵机未初始化，直接返回
    }

    // 限制角度在min_angle和max_angle之间
    if (angle < servo->min_angle)
    {
        angle = servo->min_angle;
    }
    else if (angle > servo->max_angle)
    {
        angle = servo->max_angle;
    }

    // 计算对应的脉宽（线性映射从角度到脉宽）
    float pulse_width = servo->min_pulse +
                        (angle - servo->min_angle) *
                            (float)(servo->max_pulse - servo->min_pulse) /
                            (servo->max_angle - servo->min_angle);

    // 设置脉宽
    Servo_SetPulseWidth(servo, (uint16_t)pulse_width);

    // 更新当前角度
    servo->current_angle = angle;
}

// ==================== 舵机脉宽设置函数 ====================
void Servo_SetPulseWidth(Servo_TypeDef *servo, uint16_t pulse_width_us)
{
    if (!servo->initialized)
    {
        return; // 如果舵机未初始化，直接返回
    }

    // 限制脉宽在min_pulse和max_pulse之间
    if (pulse_width_us < servo->min_pulse)
    {
        pulse_width_us = servo->min_pulse;
    }
    else if (pulse_width_us > servo->max_pulse)
    {
        pulse_width_us = servo->max_pulse;
    }

    // 计算占空比
    // 对于50Hz的PWM（周期20ms = 20000us），脉宽需要转换为0-1000范围的占空比
    uint16_t duty = (uint16_t)((float)pulse_width_us / 20.0f );

    // 设置占空比
    Servo_Duty_Setup(*(servo->htim), servo->channel, duty);
}

// ==================== 舵机速度设置函数 ====================
void Servo_SetSpeed(Servo_TypeDef *servo, uint16_t speed)
{
    servo->speed = speed;
}

// ==================== 舵机平滑移动函数 ====================
void Servo_MoveToAngle(Servo_TypeDef *servo, uint16_t target_angle)
{
    // 限制目标角度
    if (target_angle < servo->min_angle)
    {
        target_angle = servo->min_angle;
    }
    else if (target_angle > servo->max_angle)
    {
        target_angle = servo->max_angle;
    }

    // 如果当前角度与目标角度相同，不需要移动
    if (servo->current_angle == target_angle)
    {
        return;
    }

    // 根据当前角度与目标角度的关系，增加或减少当前角度
    if (servo->current_angle < target_angle)
    {
        // 需要增加角度，直到达到目标角度
        while (servo->current_angle < target_angle)
        {
            servo->current_angle += servo->speed;
            if (servo->current_angle > target_angle)
            {
                servo->current_angle = target_angle;
            }
            Servo_SetAngle(servo, servo->current_angle);
            HAL_Delay(20); // 短暂延时使舵机平滑移动
        }
    }
    else
    {
        // 需要减少角度，直到达到目标角度
        while (servo->current_angle > target_angle)
        {
            servo->current_angle -= servo->speed;
            if (servo->current_angle < target_angle)
            {
                servo->current_angle = target_angle;
            }
            Servo_SetAngle(servo, servo->current_angle);
            HAL_Delay(20); // 短暂延时使舵机平滑移动
        }
    }
}

// ==================== 舵机启动/停止函数 ====================
void Servo_Start(Servo_TypeDef *servo)
{
    HAL_TIM_PWM_Start(servo->htim, servo->channel);
}

void Servo_Stop(Servo_TypeDef *servo)
{
    HAL_TIM_PWM_Stop(servo->htim, servo->channel);
}


/* 云台舵机变量定义 */
Servo_TypeDef servo_yaw;    // 控制偏航(yaw)的舵机
Servo_TypeDef servo_pitch;  // 控制俯仰(pitch)的舵机

/**
 * @brief 初始化云台舵机
 * @param htim 定时器句柄
 * @retval None
 */
void GimbalInit(TIM_HandleTypeDef *htim)
{
    // 初始化偏航舵机(servo2)
    Servo_Init(&servo_yaw, htim, TIM_CHANNEL_2);
    
    // 初始化俯仰舵机(servo1)
    Servo_Init(&servo_pitch, htim, TIM_CHANNEL_1);
    
    // 设置云台初始位置(可根据需要调整)
    Servo_SetAngle(&servo_yaw, 90);     // 偏航舵机设置到中间位置
    Servo_SetAngle(&servo_pitch, 50);   // 俯仰舵机设置到中间位置
    
    usart_printf("云台初始化完成\r\n");
}

/**
 * @brief 控制云台摆动指定时间
 * @param duration_ms 摆动持续时间(毫秒)
 * @retval None
 */
void GimbalSweep(uint32_t duration_ms)
{
    uint32_t start_time = HAL_GetTick();
    uint32_t current_time;
    float yaw_angle = 0;
    float pitch_angle = 40;
    uint8_t yaw_direction = 1;   // 1表示增加角度，0表示减小角度
    uint8_t pitch_direction = 1; // 1表示增加角度，0表示减小角度
    uint32_t delay_ms = 20;      // 每次调整角度的延时，控制摆动速度
    
    usart_printf("开始云台摆动，持续时间: %ld ms\r\n", duration_ms);
    
    while (1)
    {
        current_time = HAL_GetTick();
        
        // 检查是否达到指定持续时间
        if (current_time - start_time >= duration_ms)
        {
            break;
        }
        
        // 更新偏航角度 (0-180度)
        if (yaw_direction)
        {
            yaw_angle += 1.0f;
            if (yaw_angle >= 180.0f)
            {
                yaw_angle = 180.0f;
                yaw_direction = 0;
            }
        }
        else
        {
            yaw_angle -= 1.0f;
            if (yaw_angle <= 0.0f)
            {
                yaw_angle = 0.0f;
                yaw_direction = 1;
            }
        }
        
        // 更新俯仰角度 (40-60度)
        if (pitch_direction)
        {
            pitch_angle += 0.5f;
            if (pitch_angle >= 60.0f)
            {
                pitch_angle = 60.0f;
                pitch_direction = 0;
            }
        }
        else
        {
            pitch_angle -= 0.5f;
            if (pitch_angle <= 40.0f)
            {
                pitch_angle = 40.0f;
                pitch_direction = 1;
            }
        }
        
        // 设置舵机角度
        Servo_SetAngle(&servo_yaw, yaw_angle);
        Servo_SetAngle(&servo_pitch, pitch_angle);
        
        // 延时以控制摆动速度
        HAL_Delay(delay_ms);
    }
    
    // 摆动结束后，将云台归位到中间位置
    Servo_SetAngle(&servo_yaw, 90);
    Servo_SetAngle(&servo_pitch, 50);
    
    usart_printf("云台摆动结束\r\n");
}
